// -*-C++-*-
#include "StateManagerService.h"

// StateProfileを複数個返す
RTMEUS::StateProfileList* StateManagerService::get_state_profiles()
{
    RTMEUS::StateProfileList_var profs = new RTMEUS::StateProfileList;
    profs->length(m_states.size());
    StateItr it;
    CORBA::ULong i;
    for(i = 0, it = m_states.begin(); it != m_states.end(); it++, i++)
    {
        RTMEUS::StateProfile_var prof = new RTMEUS::StateProfile(it->second->getStateProfile());
        profs[i] = prof;
    }
    return profs._retn();
}

RTMEUS::StateList* StateManagerService::get_states(const RTMEUS::StateNameList &names)
{
    RTMEUS::StateList_var states = new RTMEUS::StateList();
    states->length(names.length());
    for(CORBA::ULong i(0); i < names.length(); i++)
    {
        copyToState(names[i], states[i]);
    }
    return states._retn();
}

void StateManagerService::set_states(const RTMEUS::StateList &states)
{
    try {
        copyFromStateList(states);
    }
    catch(...)
    {
        std::cerr << "Error in set_states()" << std::endl;
    }
}

void StateManagerService::registerStateServiceToOutPort(RTC::PortBase* port)
{
    if (!port) {
        std::cerr << "Invalid portname for registerStateServiceToOutPort()!" << std::endl;
        return;
    }
    // propertiesに加えられないので反則技を使う const_cast
    RTC::NVList* nv = const_cast<RTC::NVList*>(&port->getPortProfile().properties);
    // PortProfileのpropertiesからさがしてなければ付け加える
    CORBA::Long index = NVUtil::find_index(*nv, "RTMEusStateService");
    RTMEUS::StateService_var service = RTMEUS::StateService::_duplicate(_this());
    if (index >= 0)
    {
        // found
        (*nv)[index].value <<= service;
    } else {
        // not found
        CORBA_SeqUtil::push_back(*nv, NVUtil::newNV("RTMEusStateService", service));
    }
}

RTMEUS::StateService_ptr StateManagerService::getStateServiceFromInPort(RTC::PortBase* port)
{
    if (!port){
        std::cerr << "Invalid portname for getStateServiceFromInPort()!" << std::endl;
        return RTMEUS::StateService::_nil();
    }
    RTC::ConnectorProfileList_var cons = port->get_connector_profiles();
    // ConnectorProfileをひとつずつ調べる
    if (cons->length() > 0) {
        for(CORBA::ULong i(0); i< cons->length(); i++) {
            if (cons[i].ports.length() > 1) {
                RTC::PortService_var port2;
                // portsに入っているどっちかが自分でどっちかが向こう
                if (port->getPortRef()->_is_equivalent(cons[i].ports[0])) port2 = cons[i].ports[1];
                else port2 = cons[i].ports[0];
                RTC::PortProfile_var prof = port2->get_port_profile();
                // RTMEusStateSerivceがあるか
                CORBA::Long index = NVUtil::find_index(prof->properties, "RTMEusStateService");
                if (index >= 0){
                    RTMEUS::StateService_ptr service;
                    prof->properties[index].value >>= service;
                    return RTMEUS::StateService::_duplicate(service);
                }
            }
        }
    }
    return RTMEUS::StateService::_nil();
}
